
import time
# from ArmIK.ArmMoveIK import *
import sys
sys.path.append('/home/ubuntu/Ai_FPV/HiwonderSDK')
import Board

if __name__ == "__main__":
    # AK = ArmIK()

    Board.setBusServoPulse(1, 291, 2500)
    Board.setBusServoPulse(2, 498, 2500)
    Board.setBusServoPulse(3, 213, 2500)
    Board.setBusServoPulse(4, 805, 2500)
    Board.setBusServoPulse(5, 424, 2500)
    Board.setBusServoPulse(6,489, 2500)
    time.sleep(4)

    Board.setBusServoPulse(1, 576, 2500)
    Board.setBusServoPulse(2, 498, 2500)
    Board.setBusServoPulse(3, 213, 2500)
    Board.setBusServoPulse(4, 805, 2500)
    Board.setBusServoPulse(5, 432, 2500)
    Board.setBusServoPulse(6,489, 2500)
    time.sleep(4)

    Board.setBusServoPulse(1, 576, 2500)
    Board.setBusServoPulse(2, 497, 2500)
    Board.setBusServoPulse(3, 213, 2500)
    Board.setBusServoPulse(4, 761, 2500)
    Board.setBusServoPulse(5,717, 2500)
    Board.setBusServoPulse(6,486, 2500)
    time.sleep(4)

    Board.setBusServoPulse(1, 576, 2500)
    Board.setBusServoPulse(2, 498, 2500)
    Board.setBusServoPulse(3, 212, 2500)
    Board.setBusServoPulse(4, 805, 2500)
    Board.setBusServoPulse(5, 453, 2500)
    Board.setBusServoPulse(6,1074, 2500)
    time.sleep(4)

    Board.setBusServoPulse(1, 289, 2500)
    Board.setBusServoPulse(2, 498, 2500)
    Board.setBusServoPulse(3, 213, 2500)
    Board.setBusServoPulse(4, 805, 2500)
    Board.setBusServoPulse(5, 453, 2500)
    Board.setBusServoPulse(6,1074, 2500)
    time.sleep(4)

    Board.setBusServoPulse(1, 289, 2500)
    Board.setBusServoPulse(2, 498, 2500)
    Board.setBusServoPulse(3, 213, 2500)
    Board.setBusServoPulse(4, 805, 2500)
    Board.setBusServoPulse(5, 523, 2500)
    Board.setBusServoPulse(6,1074, 2500)
    time.sleep(4)

    Board.setBusServoPulse(1, 289, 2500)
    Board.setBusServoPulse(2, 498, 2500)
    Board.setBusServoPulse(3, 191, 2500)
    Board.setBusServoPulse(4, 817, 2500)
    Board.setBusServoPulse(5, 727, 2500)
    Board.setBusServoPulse(6,479, 2500)
    time.sleep(4)

    Board.unloadBusServo(1, 0)

    print('\n结束\n')







